package org.usfirst.frc1305;

import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport;
/**
 * An attempt to do camera tracking independent of the regular code. 
 * 
 * It may work, or it may not.
 * 
 * @author Paul Belanger <paulirino@gmail.com>
 * @version 1.0.0
 * @since 2012-04-02
 */

public class CameraTracking extends Command {
	
	AxisCamera camera;
	ColorImage cImage;
	BinaryImage bImage;
	ParticleAnalysisReport[] report;
	
	protected void initialize() {
		camera = AxisCamera.getInstance(Constants.CAMERA_IP);
	}
	
	/**
	 * Grabs a fresh image, and operates on it. Sends a Particle report. 
	 */
	
	protected void execute() {
		while(true){
			//only perform the image if the image is fresh
			if(camera.freshImage()){
				//deal with some annoying errors
				try{
					cImage = camera.getImage();
					//isolate only the particles with a high luminance value
					bImage = cImage.thresholdHSL(0,  255, 0, 255, 190, 255);
					//perform a convex hull operation
					bImage.convexHull(true);
					//dump an image report. 
					report = bImage.getOrderedParticleAnalysisReports();
					//TODO: do something with this report. 
				}
				catch (AxisCameraException ex){
					System.out.println("Exception: " + ex.getMessage());
				}
				catch (NIVisionException ex){
					System.out.println("Exception: " + ex.getMessage());
				}
			}
		}


	}

	protected boolean isFinished() {
		// TODO Auto-generated method stub
		return false;
	}

	protected void end() {
		// TODO Auto-generated method stub

	}

	protected void interrupted() {
		// TODO Auto-generated method stub

	}

}
